import numpy as np
import cv2
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import CompressedImage

class ImageResize(Node):
    def __init__(self):
        super().__init__('image_resize')

        # 订阅原始图像话题
        self.subscription = self.create_subscription(
            CompressedImage,
            '/image',
            self.image_callback,
            10)

        # 发布压缩后图像
        self.image_resize_pub = self.create_publisher(CompressedImage, '/image_resize', 10)

    def image_callback(self, msg):
        # 将图像数据转换为OpenCV格式
        np_arr = np.frombuffer(msg.data, np.uint8)
        frame = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)

        # ============ 图像压缩为 160x120 并发布 =============
        resized_frame = cv2.resize(frame, (160, 120))
        success, encoded_image = cv2.imencode('.jpg', resized_frame)
        if success:
            compressed_msg = CompressedImage()
            compressed_msg.header = msg.header
            compressed_msg.format = "jpeg"
            compressed_msg.data = encoded_image.tobytes()
            self.image_resize_pub.publish(compressed_msg)
            self.get_logger().info("✅ 发布压缩图像到 /image_resize")
        # ====================================================

def main(args=None):  
    rclpy.init(args=args)  # 初始化rclpy  
    image_resize = ImageResize()
    rclpy.spin(image_resize)
    image_resize.destroy_node()  # 销毁节点
    rclpy.shutdown()
  

if __name__ == '__main__':  
    main()  # 入口函数
